/*
 * Created Date: Wednesday, September 15th 2021, 4:37:58 pm
 * Author: Han Hu
 * 定义一些基础函数
 * Copyright (c) 2021 Vrlab
 */

#pragma once

#include <algorithm>
#include <cstdint>
#include <map>
#include <memory>
#include <set>
#include <string>
#include <unordered_map>
#include <vector>

#include <Eigen/Core>

#ifndef M_PI
#define M_PI 3.14159265358979323846264338327950288
#endif

namespace pcv {
using namespace Eigen;

constexpr double DEG2RAD = 0.01745329251994329576923690768489;
constexpr double RAD2DEG = 57.295779513082320876798154814105;

using Matrix34d = Eigen::Matrix<double, 3, 4>;
using Matrix23d = Eigen::Matrix<double, 2, 3>;

Matrix3d rodrigues(const Vector3d &r);
Vector3d rodrigues(const Matrix3d &R);

Matrix34d P_from_KRt(const Matrix3d &K, const Matrix3d &R, const Vector3d &t);
std::tuple<Matrix3d, Matrix3d, Vector3d> KRt_from_P(const Matrix34d &P);

Matrix34d student_P(const std::vector<Vector3d> &points3d, const std::vector<Vector2d> points2d);
Matrix34d golden_P(const std::vector<Vector3d> &points3d, const std::vector<Vector2d> points2d);

std::tuple<Vector3d, Vector3d> student_PnP(const std::vector<Vector3d> &points3d, const std::vector<Vector2d> points2d,
                                           const Matrix3d &K);
std::tuple<Vector3d, Vector3d> golden_PnP(const std::vector<Vector3d> &points3d, const std::vector<Vector2d> points2d,
                                          const Matrix3d &K);

struct ProjectionData {
    std::vector<Vector3d> points3d = {Vector3d(99.1517085791261, -0.892099762666786, 3.06159510601795),
                                      Vector3d(100.558334460204, 0.868021368458366, 2.25981241694746),
                                      Vector3d(100.137647321744, -0.0612187178835884, 2.02380413900248),
                                      Vector3d(99.6742452887978, -0.675635383613515, 3.58856908136781),
                                      Vector3d(99.6224300840896, 0.0570662710124255, 2.33129745899956),
                                      Vector3d(100.203963882803, -0.474057430919711, 3.30815819695356)};

    std::vector<Vector2d> points2d = {
        Vector2d(163.209290388816, 182.199675950568), Vector2d(608.892566967667, 701.375883991155),
        Vector2d(449.262241640596, 377.206100107244), Vector2d(316.188427147902, 257.277246206713),
        Vector2d(267.575494465364, 430.054554508315), Vector2d(439.688462560083, 287.229433675101)};

    Vector2i image_size = Vector2i(800, 800);
};

} // namespace pcv
